#include "NCMain.h"
#include "../MainWindow/MainWindow.h"
#include "../main.h"
// NCCommand 类构造函数
// 接收一个字符串命令并解析
NCCommand::NCCommand(const std::string& command)
{
    parseCommand(command); // 调用解析命令函数
}
// 析构函数
NCCommand::~NCCommand()
{//缓存上个解析的点

}


// 解析命令
void NCCommand::parseCommand(const std::string& command) {
    std::istringstream iss(command); // 使用字符串流处理命令
    std::string first_code_cache; // 存储第一个代码

    cout<< command<< std::endl;
    iss >> first_code_cache; // 读取命令代码

    // 检查是否为 G 代码并调用相应的解析函数
    if (first_code_cache.empty() ||
        first_code_cache[0] == '/' ||
        first_code_cache[0] == '(' ||
        first_code_cache[0] == '#') {
        mainWindow->EXTS();

        return; // 直接返回，跳过这一行
    }
    //cout<<first_code_cache<<endl;
    if (first_code_cache[0] == 'G')
    {
        code = first_code_cache; // 保存 G 代码
        parseCommand_G(command); // 解析 G 代码
    }
    else if (first_code_cache[0] == START_CHARACTER)
    {
        mainWindow->addMessage("Starting");
        mainWindow->EXTS();
    }

    if (first_code_cache[0] == 'O')
    {

        mainWindow->addMessage(QString::fromStdString("ProgrameID: " + first_code_cache));
        mainWindow->EXTS();
    }
    else
    {
        parseCommand_M(command); // 解析 M 代码
    }
}

// 解析 G 代码
void NCCommand::parseCommand_G(const std::string& command) {
    std::istringstream iss(command); // 使用字符串流处理命令
    std::string param; // 存储参数


    while (iss >> param) {


        if (param.empty() || param.find(':') != std::string::npos || param.find('(') != std::string::npos) {
            continue; // 如果是空行或包含这些字符，则跳过此参数
        }

        if (param.length() > 1) {
            std::string key = param.substr(0, 1); // 获取参数前缀
            std::string value = param.substr(1); // 获取参数值

            // 根据参数的前缀赋值
            if (key == "X")
            {
                X = std::stod(value);
            }
            else if (key == "Y")
            {
                Y = std::stod(value);
            }

            else if (key == "Z")
            {
                Z = std::stod(value);
                cout<<"Z:"<<Z<<",";
            }

            else if (key == "F")
            {
                F = std::stod(value);
            }

            else if (key == "R")
            {
                R = std::stod(value);
            }

            else if (key == "H")
            {
                H = std::stod(value);
            }

            else if (key == "D")
            {
                D = std::stod(value);
            }

            else if (key == "I")
            {
                I = std::stod(value);  // 解析 I 参数
            }

            else if (key == "J")
            {
                J = std::stod(value);  // 解析 J 参数
            }
            cout<<endl;

        }
    }
}

// 解析 M 代码
void NCCommand::parseCommand_M(const std::string& command) {
    std::istringstream iss(command); // 使用字符串流处理命令
    std::string param; // 存储参数

    // 逐个读取参数
    while (iss >> param)
    {

        // 跳过包含 ':' 或 '(' 的参数
        if (param.empty() || param.find(':') != std::string::npos || param.find('(') != std::string::npos) {

            continue; // 如果是空行或包含这些字符，则跳过此参数

        }

        // 处理多个 M 代码
        if (param[0] == 'M')
        {
            code = param; // 保存 M 代码
        }

            // 处理刀具编号
        else if (param[0] == 'T')
        {
            Tool_num = QString::fromStdString(param.substr(1)); // 解析刀具编号为QString
        }
            // 解析主轴转速
        else if (param[0] == 'S')
        {
            S = std::stod(param.substr(1)); // 解析主轴转速
        }
        // 可以根据需要添加更多参数的处理逻辑
    }
}


// CNCStateMachine类的构造函数
// 该构造函数初始化了CNCStateMachine类的几个关键状态变量
// 没有参数和返回值，因为构造函数的目的是初始化对象
CNCStateMachine::CNCStateMachine()
// 初始化当前状态为IDLE，表示CNC机器初始处于空闲状态
        : currentState(IDLE),
        // 初始化G90_OR_G91变量为false，用于后续配置绝对或相对坐标模式
          G90_OR_G91(false),
        // 初始化刀具高度补偿值为0，表示没有初始补偿
          TOOL_H(0),
        // 初始化刀具直径补偿值为0，表示没有初始补偿
          TOOL_D(0),
        // 初始化刀具数据管理对象
        toolData()


{
    toolData.loadToolParameters();
    // 其他初始化代码
}

// 状态机事件处理
void CNCStateMachine::handleCommand(const NCCommand& command) {
    // 根据接收到的命令代码执行相应操作
    if (command.code == "G21") {
        handleG21(command);
        // 处理 G21 代码 (米制单位)
    }
    else if (command.code == "G54") {
        handleG54(command);
    }
    else if (command.code == "G90") {
        handleG90(command);
        G90_OR_G91 = false; // 绝对编程
    }
    else if (command.code == "G91") {
        G90_OR_G91 = true; // 相对编程
    }
    else if (command.code == "G43") {
        handleG43(command); // 刀具长度补偿
    }
    else if (command.code == "G44") {
        handleG44(command); // 刀具长度补偿反向
    }
    else if (command.code == "G49") {
        handleG49(command);// 刀具长度补偿取消
    }
    else if (command.code == "G40") {
        handleG40(command); // 刀具半径补偿取消
    }
    else if (command.code == "G41") {
         handleG41(command); // 左侧刀具半径补偿
    }
    else if (command.code == "G42") {
         handleG42(command); // 右侧刀具半径补偿
    }
    else if (command.code == "G00") {
        handleG00(command); // 快速定位
    }
    else if (command.code == "G01") {
        handleG01(command); // 直线插补
    }
    else if (command.code == "G02") {
        handleG02(command); // 顺时针圆弧插补
    }
    else if (command.code == "G03") {
        handleG03(command); // 逆时针圆弧插补
    }
    else if (command.code == "M00") {
        handleM00(command);// 暂停加工
    }
    else if (command.code == "M03") {
        handleM03(command); // 启动主轴顺向转
    }
    else if (command.code == "M04") {
        handleM04(command); // 启动主轴逆向转
    }
    else if (command.code == "M05") {
        handleM05(command);
    }
    else if (command.code == "M30") {
        handleM30(command);
    } else if (command.code == "M06")
    {
        std::cout << "换刀" << command.code << std::endl; // 处理未知命令
        handleM06(command); // 换刀
    }

    else {
        if( command.code.empty())
        {

        }
        else
        {
            std::cout << "未知代码: " << command.code << std::endl; // 处理未知命令
        }

    }
}

// 获取当前状态
std::string CNCStateMachine::getState() const {
    switch (currentState) {
        case IDLE:
            return "IDLE"; // 空闲状态
        case PROCESSING:
            return "PROCESSING"; // 加工中
        case PAUSED:
            return "PAUSED"; // 暂停状态
        case RAPID_MOVE:
            return "RAPID_MOVE"; // 快速移动状态
        default:
            return "UNKNOWN"; // 未知状态
    }
}

// 处理 G00 指令（快速定位）
void CNCStateMachine::handleG00(const NCCommand& cmd)
{

    if(endPoint_Z!=cmd.Z&&cmd.Z!=DEFAULT_VALUE)
        //如果Z轴发生移动,则更新Z轴位置
        //首先要排除默认值，不能把默认值搞进来
        //Z轴发生移动的条件，endPoint_Z!=cmd.Z，
        //移动Z的逻辑，从startPoint_Z移动到endPoint_Z，然后更新startPoint_Z为endPoint_Z，
    {
        endPoint_Z = cmd.Z;
        //
        //以后扩展3D用的
        //
        startPoint_Z=endPoint_Z;
        mainWindow->updateZPosition( endPoint_Z);
        mainWindow->EXTS();

    }

    moveToEndPoint(MAX_TRAVEL_SPEED, cmd);

}

// 处理 G01 指令（直线插补）
void CNCStateMachine::handleG01(const NCCommand& cmd)
{
    bool G41_OR_G42 = true;//左刀补，右刀补
    bool G40 = false;//是否有刀补
    QPointF offSet;

    if((cmd.X==DEFAULT_VALUE)&&(cmd.Y==DEFAULT_VALUE))//如果X和Y值都没有更新
    {
        if(endPoint_Z!=cmd.Z&&cmd.Z!=DEFAULT_VALUE)
            //如果Z轴发生移动,则更新Z轴位置
            //首先要排除默认值，不能把默认值搞进来
            //Z轴发生移动的条件，endPoint_Z!=cmd.Z，
            //移动Z的逻辑，从startPoint_Z移动到endPoint_Z，然后更新startPoint_Z为endPoint_Z，
        {
            endPoint_Z = cmd.Z;
            //
            //以后扩展3D用的
            //
            startPoint_Z=endPoint_Z;
            mainWindow->updateZPosition( endPoint_Z);
            mainWindow->EXTS();

        }

    }
    if((cmd.X==DEFAULT_VALUE)&&(cmd.Y==DEFAULT_VALUE)&&(cmd.Z==DEFAULT_VALUE))//如果全是默认值
    {
        mainWindow->EXTS();
    }
    readDate(cmd);//更新数据
    if(!G41_OR_G40&&!G42_OR_G40)
    {
        G40 = false;
    }
    else
    {
        G40 = true;
         if(G41_OR_G40)//左刀补
        {
            G41_OR_G42 = true;
        }
        else if(G42_OR_G40)//右刀补
        {
            G41_OR_G42 = false;
        }
    }
    if(G40==false)
    {
        offSet.setX(0);
        offSet.setY(0);
    }
    else
    {
        offSet=calculateToolOffsetTargetPoint(startPoint, QPointF(cmd.X, cmd.Y), TOOL_D/2, G41_OR_G42);

    }

    // 判断endPoint与startPoint是否不同，如果不同则执行移动
    if (startPoint.x() != endPoint.x() || startPoint.y() != endPoint.y()) {
        // 执行刀具补偿的逻辑
        endPoint=endPoint+offSet;
        mainWindow->moveToolLine(startPoint, endPoint,  speed, TOOL_D);
        // 更新startPoint为当前endPoint
        startPoint = endPoint;
        // 添加移动信息到消息框
        mainWindow->addMessage("移动到 " + QString::number(endPoint.x()) + ", " + QString::number(endPoint.y()) + "," + QString::number(endPoint_Z));
    }

}

// 处理 G02 指令（顺时针圆弧插补）
void CNCStateMachine::handleG02(const NCCommand& cmd)
{
    bool G41_OR_G42 = true;//左刀补，右刀补
    bool G40 = false;//是否有刀补
    QPointF offSet;
    if(endPoint_Z!=cmd.Z&&cmd.Z!=DEFAULT_VALUE)
        //如果Z轴发生移动,则更新Z轴位置
        //首先要排除默认值，不能把默认值搞进来
        //Z轴发生移动的条件，endPoint_Z!=cmd.Z，
        //移动Z的逻辑，从startPoint_Z移动到endPoint_Z，然后更新startPoint_Z为endPoint_Z，
    {
        endPoint_Z = cmd.Z;
        //
        //以后扩展3D用的
        //
        startPoint_Z=endPoint_Z;
        mainWindow->updateZPosition( endPoint_Z);
        mainWindow->EXTS();

    }
    readDate(cmd);
    if(!G41_OR_G40&&!G42_OR_G40)
    {
        G40 = false;
    }
    else
    {
        G40 = true;
        if(G41_OR_G40)//左刀补
        {
            G41_OR_G42 = true;
        }
        else if(G42_OR_G40)//右刀补
        {
            G41_OR_G42 = false;
        }
    }
    if(G40==false)
    {
        offSet.setX(0);
        offSet.setY(0);
    }
    else
    {
        if(G41_OR_G42)//左刀补
        {
            offSet=calculateToolOffsetTargetPointArc(endPoint, centerPoint, TOOL_D/2, true);
        }
        else//右刀补
        {
            offSet=calculateToolOffsetTargetPointArc(endPoint, centerPoint, TOOL_D/2, false);

        }

    }

    endPoint=endPoint+offSet;
    mainWindow->moveToolArc(startPoint, endPoint, centerPoint, speed, TOOL_D);
    startPoint=endPoint;
    mainWindow->addMessage("移动到 " + QString::number(endPoint.x()) + ", " + QString::number(endPoint.y()) + "," + QString::number(endPoint_Z));
}

// 处理 G03 指令（逆时针圆弧插补）
void CNCStateMachine::handleG03(const NCCommand& cmd) {
    bool G41_OR_G42 = true;//左刀补，右刀补
    bool G40 = false;//是否有刀补
    QPointF offSet;
    if(endPoint_Z!=cmd.Z&&cmd.Z!=DEFAULT_VALUE)
        //如果Z轴发生移动,则更新Z轴位置
        //首先要排除默认值，不能把默认值搞进来
        //Z轴发生移动的条件，endPoint_Z!=cmd.Z，
        //移动Z的逻辑，从startPoint_Z移动到endPoint_Z，然后更新startPoint_Z为endPoint_Z，
    {
        endPoint_Z = cmd.Z;
        //
        //以后扩展3D用的
        //
        startPoint_Z=endPoint_Z;
        mainWindow->updateZPosition( endPoint_Z);
        mainWindow->EXTS();

    }
    readDate(cmd);
    if(!G41_OR_G40&&!G42_OR_G40)
    {
        G40 = false;
    }
    else
    {
        G40 = true;
        if(G41_OR_G40)//左刀补
        {
            G41_OR_G42 = true;
        }
        else if(G42_OR_G40)//右刀补
        {
            G41_OR_G42 = false;
        }
    }
    if(G40==false)
    {
        offSet.setX(0);
        offSet.setY(0);
    }
    else
    {
        if(G41_OR_G42)//左刀补
        {
            offSet=calculateToolOffsetTargetPointArc(endPoint, centerPoint, TOOL_D/2, false);
        }
        else//右刀补
        {
            offSet=calculateToolOffsetTargetPointArc(endPoint, centerPoint, TOOL_D/2, true);

        }

    }

    endPoint=endPoint+offSet;
    mainWindow->moveToolArc(startPoint, endPoint, centerPoint, speed, TOOL_D);
    startPoint=endPoint;
    mainWindow->addMessage("移动到 " + QString::number(endPoint.x()) + ", " + QString::number(endPoint.y()) + "," + QString::number(endPoint_Z));
}


// 处理G40指令的函数
// G40指令表示取消刀具半径补偿
// 参数cmd: 包含G40指令的NC命令对象
void  CNCStateMachine::handleG40(const NCCommand &cmd)
{
    G41_OR_G40 = false;//左侧刀具半径补偿
    G42_OR_G40 = false;//右侧刀具半径补偿
}

// 处理 G41 指令（左侧刀具半径补偿）
void CNCStateMachine::handleG41(const NCCommand& cmd)
{
     G41_OR_G40 = true;//左侧刀具半径补偿
     G42_OR_G40 = false;//右侧刀具半径补偿
    mainWindow->addMessage("左刀具补偿");
    mainWindow->EXTS();//读取下一步代码的触发信号
}

/**
 * 处理 G42 指令（右侧刀具半径补偿）
 * 该指令用于右侧刀具半径补偿，返回刀具半径的负值以实现补偿。
 */
void CNCStateMachine::handleG42(const NCCommand& cmd)
{
    G41_OR_G40 = false;//左侧刀具半径补偿
    G42_OR_G40 = true;//右侧刀具半径补偿

    mainWindow->addMessage("右侧刀具补偿");
    mainWindow->EXTS();//读取下一步代码的触发信号
}

// 刀具长度补偿
/**
 * 处理G43指令，该指令用于刀具长度正补偿。
 * 当执行G43指令时，机床会根据当前刀具的长度补偿值，调整刀具相对于工件的高度。
 */
void CNCStateMachine::handleG43(const NCCommand& cmd)
{

    G43_OR_G49 = true;//正向补偿
    G44_OR_G49 = false;//负向补偿
    endPoint_Z=cmd.Z+TOOL_H;

   mainWindow-> updateZPosition(endPoint_Z);
   //后期改3D

   //
   startPoint_Z=endPoint_Z;

    mainWindow->addMessage("刀具长度正向补偿");
    mainWindow->EXTS();//读取下一步代码的触发信号

}
/**
 * 处理G44指令，实现刀具长度补偿反向
 *
 * @param cmd NC命令对象，包含了指令的相关信息
 * @return 返回刀具长度补偿值的相反数
 *
 * G44指令通常用于数控编程中进行刀具长度补偿。本函数通过取NC命令中H参数的相反数，
 * 实现了刀具长度补偿的反向操作。这种设计允许在不同的加工场景下灵活调整刀具的补偿方向。
 */
void CNCStateMachine::handleG44(const NCCommand& cmd)
{

    G43_OR_G49 = false;//正向补偿
    G44_OR_G49 = true;//负向补偿
    endPoint_Z=cmd.Z-TOOL_H;
    mainWindow-> updateZPosition(endPoint_Z);
    //后期改3D

    //
    startPoint_Z=endPoint_Z;
    mainWindow->addMessage("刀具长度负向补偿");
    mainWindow->EXTS();//读取下一步代码的触发信号
}
void CNCStateMachine::handleG49(const NCCommand &cmd)
{

     G43_OR_G49 = false;//正向补偿
     G44_OR_G49 = false;//负向补偿
    mainWindow->addMessage("刀具长度补偿取消");
    mainWindow->EXTS();//读取下一步代码的触发信号
}

void CNCStateMachine::handleG21(const NCCommand &cmd)
{
    mainWindow->addMessage("设置单位为公制，毫米");
    mainWindow->EXTS();

};
void CNCStateMachine::handleG90(const NCCommand &cmd)
{
    mainWindow->addMessage("绝对坐标编程");
    mainWindow->EXTS();
};
void CNCStateMachine::handleG54(const NCCommand &cmd)
{
    mainWindow->addMessage("选择第一个工件坐标系");
    mainWindow->EXTS();

}
// 启动加工
void CNCStateMachine::handleM03(const NCCommand &cmd) {

    if (currentState == IDLE || currentState == PAUSED) {
        mainWindow->addMessage("启动主轴顺时针旋转");

        currentState = PROCESSING;
    }
    else
    {
        mainWindow->addMessage("修改主轴转速为"+QString::number(cmd.S));
    }
    mainWindow->updateSpindleSpeed(cmd.S);
    mainWindow->EXTS();
}
void CNCStateMachine::handleM04(const NCCommand &cmd)
{
    if (currentState == IDLE || currentState == PAUSED) {
        mainWindow->addMessage("启动主轴逆时针旋转");

        currentState = PROCESSING;
    }
    else
    {
        mainWindow->addMessage("修改主轴转速为"+QString::number(-cmd.S));
    }
    mainWindow->updateSpindleSpeed(-cmd.S);
    mainWindow->EXTS();
}
// 停止加工
void CNCStateMachine:: handleM05(const NCCommand &cmd) {
    if (currentState == PROCESSING) {
        cout << "停止主轴，进入空闲状态。" << endl;
        currentState = IDLE;
    }
    else {
        cout << "当前不在加工状态中，无法停止主轴。" << endl;
    }
}

// 暂停加工
void CNCStateMachine::handleM00(const NCCommand &cmd) {
    if (currentState == PROCESSING) {
        cout << "暂停加工，进入暂停状态。" << endl;
        currentState = PAUSED;
    }
    else {
        cout << "当前不在加工状态中，无法暂停。" << endl;
    }
}

// 程序重置
void CNCStateMachine:: handleM30(const NCCommand &cmd){
    cout << "程序结束并返回开头。" << endl;
    currentState = IDLE;
}

//换刀程序
void CNCStateMachine:: handleM06(const NCCommand &cmd)
{
    currentToolID=cmd.Tool_num;
     TOOL_H = toolData.getToolLength( currentToolID);
     TOOL_D = toolData.getToolRadius(currentToolID);
    mainWindow->addMessage("换刀到工具编号："+currentToolID+ "刀具长度："+QString::number(TOOL_H)+"刀具半径："+QString::number(TOOL_D));
    mainWindow->EXTS();

}
// 函数：moveToEndPoint
// 说明：根据输入的速度和命令，判断是否需要移动刀具到新的终点位置。
// 参数：
// - speed：移动速度，单位为毫米每分钟。
// - cmd：包含刀具目标位置的NCCommand对象。
//XY平面的移动逻辑，判断end的值是否发生变化，变化了才移动
//endPoint的值由cmd.X和Y得到
//也要避免默认值传经来
//判断cmd.X和Y是不是默认值
//移动逻辑，startPoint，移动到endPoint，然后更新startPoint为endPoint
void CNCStateMachine:: moveToEndPoint(double speed, const NCCommand& cmd) {

    readDate(cmd);//更新数据

    // 判断endPoint与startPoint是否不同，如果不同则执行移动
    if (startPoint.x() != endPoint.x() || startPoint.y() != endPoint.y()) {
        // 执行快速定位逻辑
        mainWindow->moveToolLine(startPoint, endPoint, speed, TOOL_D);
        // 更新startPoint为当前endPoint
        startPoint = endPoint;
        // 添加移动信息到消息框
        mainWindow->addMessage("移动到 " + QString::number(endPoint.x()) + ", " + QString::number(endPoint.y()) + "," + QString::number(endPoint_Z));
    }
}
// 计算刀补后目标点的差值
/**
 * @brief 计算刀具偏移向量
 *
 * 该函数根据起始点和结束点之间的方向，以及刀具半径计算刀具的偏移目标点。
 * 支持 G41 和 G42 刀具偏移模式。
 *
 * @param Pointstart 起始点
 * @param Pointend 结束点
 * @param R 刀具半径
 * @param G41_OR_G42 刀具偏移模式标识，true 为 G41，false 为 G42
 * @return QPointF 返回计算后的刀具偏移目标点
 */
QPointF CNCStateMachine::calculateToolOffsetTargetPoint(const QPointF &Pointstart, const QPointF &Pointend, double R, bool G41_OR_G42)
{
    QPointF direction = Pointend - Pointstart; // 计算起始点和结束点之间的方向向量
    QPointF centerEdgeOffset; // 中心边缘偏移

    // 计算直线的斜率和角度
    if ((direction.x() != 0) && (direction.y() != 0)) {
        double Line_K = direction.y() / direction.x(); // 计算斜率
        double angleInRadians = std::atan2(direction.y(), direction.x()); // 计算与X轴的夹角

        // 使用 angleInRadians 计算中心偏移值
        centerEdgeOffset.setX(std::abs(R * sin(angleInRadians)));
        centerEdgeOffset.setY(std::abs(R * cos(angleInRadians)));
    } else if (direction.x() == 0) {
        centerEdgeOffset.setX(std::abs(R)); // 竖直方向，偏移量为R
        centerEdgeOffset.setY(0);
    } else if (direction.y() == 0) {
        centerEdgeOffset.setX(0);
        centerEdgeOffset.setY(std::abs(R)); // 水平方向，偏移量为R
    }

    // 根据刀具直线运动方向计算刀具边缘偏移
    QPointF leftCenterEdgeOffset, rightCenterEdgeOffset;

    if (direction.x() > 0) { // 向右运动
        if (direction.y() > 0) { // 第一象限
            leftCenterEdgeOffset = QPointF(centerEdgeOffset.x(), -centerEdgeOffset.y()); // 左边缘偏移
            rightCenterEdgeOffset = QPointF(-centerEdgeOffset.x(), centerEdgeOffset.y()); // 右边缘偏移
        } else if (direction.y() < 0) { // 第四象限
            leftCenterEdgeOffset = QPointF(-centerEdgeOffset.x(), -centerEdgeOffset.y());
            rightCenterEdgeOffset = QPointF(centerEdgeOffset.x(), centerEdgeOffset.y());
        } else { // 水平运动
            leftCenterEdgeOffset = QPointF(0, -centerEdgeOffset.y());
            rightCenterEdgeOffset = QPointF(0, centerEdgeOffset.y());
        }
    } else if (direction.x() < 0) { // 向左运动
        if (direction.y() > 0) { // 第二象限
            leftCenterEdgeOffset = QPointF(centerEdgeOffset.x(), centerEdgeOffset.y());
            rightCenterEdgeOffset = QPointF(-centerEdgeOffset.x(), -centerEdgeOffset.y());
        } else if (direction.y() < 0) { // 第三象限
            leftCenterEdgeOffset = QPointF(-centerEdgeOffset.x(), centerEdgeOffset.y());
            rightCenterEdgeOffset = QPointF(centerEdgeOffset.x(), -centerEdgeOffset.y());
        } else { // 水平运动
            leftCenterEdgeOffset = QPointF(0, centerEdgeOffset.y());
            rightCenterEdgeOffset = QPointF(0, -centerEdgeOffset.y());
        }
    } else { // 竖直运动
        if (direction.y() > 0) { // 向下运动
            leftCenterEdgeOffset = QPointF(centerEdgeOffset.x(), 0.0);
            rightCenterEdgeOffset = QPointF(-centerEdgeOffset.x(), 0.0);
        } else if (direction.y() < 0) { // 向上运动
            leftCenterEdgeOffset = QPointF(-centerEdgeOffset.x(), 0);
            rightCenterEdgeOffset = QPointF(centerEdgeOffset.x(), 0);
        } else { // 静止状态
            leftCenterEdgeOffset = QPointF(0, 0);
            rightCenterEdgeOffset = QPointF(0, 0);
        }
    }

    // 返回根据 G41/G42 选择的刀具偏移
    return G41_OR_G42 ? leftCenterEdgeOffset : rightCenterEdgeOffset;
}
QPointF CNCStateMachine::calculateToolOffsetTargetPointArc(const QPointF &point, const QPointF &pointCenter, double radius, bool isOuterCircle)
{
    cout<<"("<<point.x()<<","<<point.y()<<")"<<endl;
    cout<<"("<<pointCenter.x()<<","<<pointCenter.y()<<")"<<endl;
    cout<<radius<<endl;
    // 计算起点向量和起始角度
    QPointF startVec = point - pointCenter;
    double startAngle;

    if (startVec.x() == 0 && startVec.y() == 0) {
        startAngle = 0; // 默认角度
    } else {
        startAngle = std::atan2(startVec.y(), startVec.x());
        startAngle = (startAngle < 0) ? (startAngle + 2 * M_PI) : startAngle; // 确保在 0 到 2π 范围内
    }

    // 计算偏移
    QPointF offset;
    QPointF centerEdgeOffset;
    offset.setX(std::abs(radius * sin(2*M_PI-startAngle)));
    offset.setY(std::abs(radius * cos(2*M_PI-startAngle)));

    // 根据起始角度和是大圈还是小圈计算中心边缘偏移
    if (isOuterCircle) {
        if (startAngle == 0) centerEdgeOffset = QPointF(radius, 0);
        else if (startAngle == M_PI) centerEdgeOffset = QPointF(-radius, 0);
        else if (startAngle == M_PI / 2) centerEdgeOffset = QPointF(0, radius);
        else if (startAngle == 3 * M_PI / 2) centerEdgeOffset = QPointF(0, -radius);
        else if (startAngle < M_PI / 2) centerEdgeOffset = QPointF(offset.x(), offset.y()); // 第一象限
        else if (startAngle < M_PI) centerEdgeOffset = QPointF(-offset.x(), offset.y()); // 第二象限
        else if (startAngle < 3 * M_PI / 2) centerEdgeOffset = QPointF(-offset.x(), -offset.y()); // 第三象限
        else centerEdgeOffset = QPointF(offset.x(), -offset.y()); // 第四象限
    } else {
        if (startAngle == 0) centerEdgeOffset = QPointF(-radius, 0);
        else if (startAngle == M_PI) centerEdgeOffset = QPointF(radius, 0);
        else if (startAngle == M_PI / 2) centerEdgeOffset = QPointF(0, -radius);
        else if (startAngle == 3 * M_PI / 2) centerEdgeOffset = QPointF(0, radius);
        else if (startAngle < M_PI / 2) centerEdgeOffset = QPointF(-offset.x(), -offset.y()); // 第一象限
        else if (startAngle < M_PI) centerEdgeOffset = QPointF(offset.x(), -offset.y()); // 第二象限
        else if (startAngle < 3 * M_PI / 2) centerEdgeOffset = QPointF(offset.x(), offset.y()); // 第三象限
        else centerEdgeOffset = QPointF(-offset.x(), offset.y()); // 第四象限
    }

    return centerEdgeOffset;
}
void CNCStateMachine::readDate(const NCCommand& cmd)
{
    // 检查cmd.X是否为默认值，如果不是则更新endPoint的X坐标
    if (cmd.X != DEFAULT_VALUE) {
        endPoint.setX(cmd.X);
    }
    // 检查cmd.Y是否为默认值，如果不是则更新endPoint的Y坐标
    if (cmd.Y != DEFAULT_VALUE) {
        endPoint.setY(cmd.Y);
    }
    // 检查cmd.Z是否为默认值，如果不是则更新endPoint_Z
    if (cmd.Z != DEFAULT_VALUE) {
        endPoint_Z = cmd.Z;
    }
    //检查进给速度是否为默认值，如果不是则更新speed
    if (cmd.F != DEFAULT_VALUE) {
        speed=cmd.F;
    }
    //检查中心点I和J是否为默认值，如果不是则更新centerPoint
    if (cmd.I != DEFAULT_VALUE) {
        centerPoint.setX(cmd.I);
    }
    if (cmd.J != DEFAULT_VALUE) {
        centerPoint.setY(cmd.J);

    }

}
void GetToolData::loadToolParameters() {
    QFile file("tools.txt");  // 定义文件名为 "tools.txt"

    if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) {
        cout << "无法打开刀具文件" << endl;
        return;
    }

    QTextStream in(&file);
    while (!in.atEnd()) {
        QString line = in.readLine();  // 读取一行内容
        QStringList params = line.split(";");  // 以分号分割参数

        // 检查参数数量是否大于等于 4
        if (params.size() >= 4) {
            ToolParameters toolParams;  // 创建一个 ToolParameters 对象

            // 解析每个参数并去掉多余的空格
            for (const QString &param : params) {
                QStringList keyValue = param.split(":");
                if (keyValue.size() == 2) {
                    QString key = keyValue[0].trimmed();
                    QString value = keyValue[1].trimmed();

                    // 根据键将值存入对应的成员变量
                    if (key == "ID") {
                        toolParams.toolID = value;
                    } else if (key == "diameter") {
                        toolParams.radius = value.toDouble();  // 转换为 double
                    } else if (key == "length") {
                        toolParams.length = value.toDouble();  // 转换为 double
                    }
                    // 可以添加其他参数的解析
                }
            }

            // 将刀具参数存入 QMap，使用刀具ID作为键
            toolParametersMap.insert(toolParams.toolID, toolParams);
        }
    }

    file.close();  // 关闭文件
}


// 获取指定刀具的半径。
double GetToolData::getToolRadius(const QString &toolID) {
    // 检查刀具参数映射中是否包含指定的刀具ID。
    if (toolParametersMap.contains(toolID)) {
        // 返回刀具的半径。
        return toolParametersMap[toolID].radius;
    }
    // 如果刀具ID不存在，则返回 -1。
    return -1; // 或者抛出异常
}

// 获取指定刀具的长度。
double GetToolData::getToolLength(const QString &toolID) {
    // 检查刀具参数映射中是否包含指定的刀具ID。
    if (toolParametersMap.contains(toolID)) {
        // 返回刀具的长度。
        return toolParametersMap[toolID].length;
    }
    // 如果刀具ID不存在，则返回 -1。
    return -1; // 或者抛出异常
}


